#include <stdio.h>
#include <math.h>

#include <util/msg.h>

#include "filters.h"

#include "../../matrix/matrix.h"

ImpMatrix * matrix_create_filter_gaussian(int size, real s) {
	ImpMatrix * Gauss;
	real * gauss;

	int rows, columns;
	real val, sum;

	real x, y, ux, uy, sx, sy;
	real ex;

	int r, c, _r, i;

	rows = size;
	columns = size;

	Gauss = imp_matrix_new(rows, columns);
	gauss = Gauss->data;

	ux = (size - 1) / 2;
	uy = (size - 1) / 2;

	sx = s;
	sy = s;

	sum = 0;
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;

			x = c;
			y = r;


			ex = (x - ux)*(x - ux) / (2 * sx * sx) + (y - uy)*(y - uy) / (2 * sy * sy);

			val = exp(-ex);

			gauss[i] = val;
			sum = sum + val;
		}
	}
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;
			gauss[i] = gauss[i] / sum;
		}
	}
	return Gauss;
}

ImpMatrix * imp_filter_new_gaussian(int size, real s) {
	ImpMatrix * Gauss;
	real * gauss;

	int rows, columns;
	real val, sum;

	real x, y, ux, uy, sx, sy;
	real ex;

	int r, c, _r, i;

	rows = size;
	columns = size;

	Gauss = imp_matrix_new(rows, columns);
	gauss = Gauss->data;

	ux = (size - 1) / 2;
	uy = (size - 1) / 2;

	sx = s;
	sy = s;

	sum = 0;
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;

			x = c;
			y = r;


			ex = (x - ux)*(x - ux) / (2 * sx * sx) + (y - uy)*(y - uy) / (2 * sy * sy);

			val = exp(-ex);

			gauss[i] = val;
			sum = sum + val;
		}
	}
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;
			gauss[i] = gauss[i] / sum;
		}
	}
	return Gauss;
}

ImpMatrix * matrix_create_filter_gaussian_teta(int size, real sx, real sy, real teta) {
	ImpMatrix * Gauss;
	real * gauss;

	int rows, columns;
	real val, sum;

	real x, y, ux, uy;
	real ex;

	real cost, sint;

	real A, B, C;

	int r, c, _r, i;

	rows = size;
	columns = size;

	Gauss = imp_matrix_new(rows, columns);
	gauss = Gauss->data;

	ux = (size - 1) / 2;
	uy = (size - 1) / 2;

	cost = cos(teta);
	sint = sin(teta);

	A = ((cost * cost) / (2 * sx * sx)) + ((sint * sint) / (2 * sy * sy));
	B = -(sin(2 * teta) / (4 * sx * sx)) + (sin(2 * teta) / (4 * sy * sy));
	C = ((sint * sint) / (2 * sx * sx)) + ((cost * cost) / (2 * sy * sy));

	sum = 0;
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;

			x = c - ux;
			y = r - uy;


			ex = A*x*x + 2*B*x*y + C*y*y;

			val = exp(-ex);

			gauss[i] = val;
			sum = sum + val;
		}
	}
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;
			gauss[i] = gauss[i] / sum;
		}
	}
	return Gauss;
}


ImpMatrix * imp_filter_new_gaussian_full(int rows, int columns, real sx, real sy, real teta) {
	
	ImpMatrix * Gauss = NULL;
	real * gauss;

	real x, y, ux, uy;
	real ex;
	real cost, sint;
	real A, B, C;
	real val, sum;
	
	int r, c, _r, i;
	
	Gauss = imp_matrix_new(rows, columns);
	gauss = Gauss->data;

	ux = (columns - 1) / 2;
	uy = (rows - 1) / 2;

	cost = cos(teta);
	sint = sin(teta);

	A = ((cost * cost) / (2 * sx * sx)) + ((sint * sint) / (2 * sy * sy));
	B = -(sin(2 * teta) / (4 * sx * sx)) + (sin(2 * teta) / (4 * sy * sy));
	C = ((sint * sint) / (2 * sx * sx)) + ((cost * cost) / (2 * sy * sy));

	sum = 0;
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;

			x = c - ux;
			y = r - uy;

			ex = A*x*x + 2*B*x*y + C*y*y;

			val = exp(-ex);

			gauss[i] = val;
			sum = sum + val;
		}
	}
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;
			gauss[i] = gauss[i] / sum;
		}
	}
	return Gauss;
}